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ABSTRACT 



It is generally agreed that some type of vision system capable of providing a 
preview of terrain is an important attribute of a driverless vehicle. One approach 
to providing this capability is to use active images such as those obtained by 
sonar or radar. This thesis is concerned with a computer simulation study of an 
approach to data processing for range images obtained from an optical radar 
system using a scanning laser beam. The system studied is modeled after the 
ERIM scanner mounted on the Adaptive Suspension Vehicle walking machine 
developed at Ohio State University. Both the problem of registering successive 
images in the presence of vehicle motion and of optimally averaging such images 
to obtain more accurate terrain elevation data are investigated in the thesis. 
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I. INTRODUCTION 



Robots have long been in man’s thinking. From the early science fiction 
writers to today’s cartoons, the idea of robots that operate on their own has 
fascinated us. There have been, and still are, many research programs concerned 
with realization of an autonomous robot. These programs include wheeled 
machines, tracked vehicles, and legged vehicles. 

There are many uses and advantages to each type of robotic machine. Legged 
vehicles .or walking machines have am advantage over wheeled or tracked vehicles 
in that they can traverse a greater variety of terrains. A walking machine can 
traverse terrain that is heavily wooded, can walk over muddy terrain, and can 
negotiate obstacles that a wheeled or tracked vehicle may not be able to 
overcome. However, before any robotic machine can travel anywhere by itself, it 
has to be able to see where it is going and decide where it wants to go based on 
what it sees. Once all of these research areas have been mastered, widespread 
application of autonomous robots may follow. 

A. GOALS 

The goal of this thesis is to explore methods to obtain accurate terrain 
altitude information that may be used by some type of route planning program 
for an autonomous vehicle. The process of obtaining these altitude values will 
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include some type of filtering of noisy information and correction of errors in the 
vehicle’s location information. The sensors involved in this process are assumed 
to be some type of inertial navigation system (INS) and an optical radar scanner. 

The vehicle used as a reference for setting up the model used in this thesis is 
the Ohio State University Adaptive Suspension Vehicle (ASV), which is a six- 
legged walking machine. The ASV is equipped with an optical terrain scanner 
manufactured by the Environmental Research Institute of Michigan (ERIM). 
[Ref. 1] 

A secondary purpose of this thesis is to develop a simulation model for the 
optical scanner and terrain that will be used in the testing of the techniques 
presented in the thesis. These models could also be used in future studies that 
involve a scanner and terrain. 

B. ORGANIZATION 

Chapter II presents a discussion of work dealing with early robotic systems as 
well as current projects under development. The optical terrain scanner used as a 
reference in this thesis is discussed in some detail. Finally, a general discussion on 
regression analysis and Kalman filtering are presented. 

The reason for obtaining an accurate terrain altitude map is discussed in 
Chapter III. A detailed discussion is presented on how the inertial navigation and 
optical scanner data are mathematically manipulated to obtain the x and y 
coordinates and the terrain altitude of the terrain being scanned. Finally, a more 
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detailed discussion of Kalman filtering and regression analysis as they apply to 
this thesis are presented. 

Chapter IV presents the simulation models used for the terrain and the 
optical scanner. Also, flow charts showing how data is handled during the 
simulation runs are included. 

Finally, Chapter V discusses the simulation experiments conducted and their 
results. The last chapter, Chapter VI, puts forward some conclusions about the 
work presented in this thesis followed by some recommendations for future work. 
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II. REVIEW OF PREVIOUS WORK 



A. INTRODUCTION 

For many years, man has been working toward the goal of making an 
autonomous robot. However, before a robot or machine can be truly autonomous, 
it has to have some way of seeing the world in which it is to operate. In this 
chapter, past robotic systems and their vision systems are reviewed along with 
ways of handling the data from these vision systems. 

B. MOBILE ROBOT SYSTEMS 

1. Early Robot Systems 

Inventors have been fascinated for centuries with the idea of creating 
machines that act like animals or human beings. The earliest of these were 
"clockwork" machines like the "artificial duck" built by Jacques de Vaucanson in 
the 1730’s, and Baron Wolfgang von Kempenlen’s chess-playing automaton of the 
late eighteenth century (later proved a hoax). Clockwork machines, as the name 
implies, were based on complex mechanical gears and linkages which provided the 
timing and required movements for the machine. Until the advent of digital 
computers and component miniaturization technology, robots remained little 
more than mechanical novelties. [Ref. 2: pp. 254-264] 
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In the 1960’s, researchers at Johns Hopkins University built what could 
possibly be considered the first truly autonomous mobile robot. The sole activity 
of this robot was to navigate the hallways of the laboratory "searching" for 
electrical power outlets to keep its batteries charged. Special purpose circuitry 
(rather than a programmable computer) was used to direct the robot’s actions, 
and information from an elementary sonar system kept the robot centered 
between the walls. Although simplistic, the machine demonstrated an ability to 
interact with its enviroment and to sustain itself through its own actions (i.e. 
searching for outlets and recharging its batteries). [Ref. 2: pp. 254-264] 

The next major step in autonomous robot development was accomplished 
in 1969 at the Stanford Research Institute (SRI). A mobile robot dubbed 
"Shakey" was linked to a digital computer via radio giving it the much needed 
additional "brainpower" to more fully interact with its enviroment and solve 
relatively complex problems. Using a television camera as a sensor and 
hierarchical software control, Shakey was able to negotiate obstacles and 
accomplish simple path planning when moving from one location to another. 
This first attempt at machine interaction with an uncertain enviroment 
encountered many problems. Rather complex image processing algorithms were 
required to discern potential obstacles and complicated computer programs were 
needed to map the obstacles onto a spatial grid coordinate system that the robot 
could "understand". Shakey kept track of its own position through a crude "dead 
reckoning" system in which sensors counted the number of rotations of the robot’s 
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wheels to determine how far it had moved from its initial starting position. It was 
soon discovered that, while multiple sensors can provide a more comprehensive 
representation of the enviroment, coordinating the incoming data from those 
sensors represents a major hurdle. For instance, on the Shakey robot, wheel 
slippage caused position errors which in turn induced grid map drift and resulted 
in multiple representations of individual obstacles cluttering the map. [Ref. 3: pp. 
10-15] 

For all its problems, the Shakey robot remained state-of-the-art until the 
mid-1970’s. During this decade, research involving incorporation of new 
technologies into autonomous robots continued. Advances in integrated 
electronics manufacturing made possible relatively powerful single-board 
computers and smaller, more compact sensor systems. Still, many of the robots 
built during this time frame were tethered to an off-board computer either via 
cables or radio link. Among the robots of this period were the Jet Propulsion 
Laboratory’s Mars Rover, the Stanford Cart, and France’s Laboratoire 
d’Automatique et d’Analyse des Systemes’ HILARE robot. These robots all 
shared the common goal of integrating multiple sensor systems and developing 
algorithms to allow autonomous operation in uncertain enviroments. [Ref. 3: pp. 
20-25] 

The JPL Rover used a laser range finder, stereo TV cameras, tactile, and 
proximity sensors to observe its enviroment, and a gyrocompass and optical 
encoders on the wheels to keep track of its own position. Like the Shakey robot, 



12 



sensor integration and error propogation problems plagued the system and, due to 
the heavy computing requirements, the robot was dependent on its mainframe 
computer link. [Ref. 3: pp. 20-30] 

The Stanford Cart employed a complex image processing scheme to 
improve obstacle recognition. A TV link to an off-board computer and a 
moveable, slide-mounted camera system were used to reduce imaging errors, but 
this system proved slow and very sensitive to light and shadow effects. Like 
previous robots, the Stanford Cart’s dead reckoning system induced errors into 
the system which it could not overcome. [Ref. 3: pp. 20-25] 

The French Hilare robot incorporated a number of techniques most often 
associated with artificial intelligence. "Expert system" modules worked together, 
sharing information about navigation, obstacle identification, etc., under a 
computing hierarchy of onboard microcomputers linked to an off-board 
minicomputer and mainframe. The Hilare used a laser range finder and a 
prediction algorithm to anticipate what the obstacle map would "look" like after 
the robot moved. It could then correct its perception and/or position information 
to merge the predicted and actual image information and thereby alleviate many 
of the errors experienced in the earlier systems. [Ref. 3: pp. 20-25] 

A departure from wheeled vehicles is the Ohio State University (OSU) 
Hexapod. The Hexapod is a six- legged walking machine which first operated in 
1977. Although the OSU Hexapod is strictly a laboratory robot, a truly 
autonomous walking machine, like its animal counterpart in nature, would be 



13 



ideally suited to traversing rough, unpredictable terrain. The OSU machine’s legs 
are controlled to provide static stability; that is, at least three legs are on the 
ground providing a stable supporting tripod for the robot at all times. The 
robot’s sensors include stereo TV cameras and motion sensors to maintain stable 
body position while moving or climbing. While sophisticated computer routines 
automate many of the Hexapod’s balance, leg movement, and coordination 
functions, the Hexapod remains tethered to its external power supply and 
minicomputer and relies on human interaction for navigation and for some foot- 
placement decisions. [Ref. 4: pp. 3-17] 

2. Autonomous Land Vehicles 

The Autonomous Land Vehicle (ALV) is the newest, and thus far the 
most successful, of the various autonomous wheeled robots which have been tested 
to date. Designed for the Defense Department’s Advanced Research Projects 
Agency (DARPA), the vehicle is a hydraulically powered, eight- wheeled platform 
equipped with the computers and sensors required to negotiate unfamiliar 
territory without human intervention. The ALV’s mission is conceptually more 
difficult than the robots previously described since it is designed to operate 
outdoors in a much more unpredictable enviroment. The ALV’s sensors include a 
TV camera and optical radar for sensing the enviroment, and an inertial 
navigation system for position information. During initial testing, the sensor data 
was successfully integrated and processed, providing the vehicle with "road 
following" information able to keep it between ditches. [Ref 5] 
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Another ALV was built in 1985 by FMC Corporation. This vehicle is an 
armored personnel carrier which is a tracked, instead of wheeled vehicle. This 
ALV has an inertial navigation system, a vehicle control computer, and a sonic 
✓ imaging sensor. The architecture of the FMC vehicle consists of 5 subsystems 
called a Planner, Observer, Mapmaker, Pilot, and Vehicle Control. In general, 
the Planner plans the route of the ALV using preloaded digitized maps of the 
local terrain. The Observer uses the sensors input to create an obstacle map, then 
the Mapmaker generates a pilot map using the information from the Planner and 
the obstacle map. The Pilot uses the pilot map and guides the vehicle along an 
optimum path by passing instructions to the Vehicle Control subsystem. The 
first test of the FMC ALV was conducted in 1985 in which it successfully avoided 
obstacles and performed path execution at a speed of 5 mph. [Ref. 6: pp. 14-23] 

3. DARPA Adaptive Suspension Vehicle 

A more advanced six-legged walking machine, called the Adaptive 
Suspension Vehicle (ASV), is currently undergoing test and evaluation at Ohio 
State University. The ASV differs from the OSU Hexapod in that it is completely 
self-sufficient (no external power or computing required). The ASV uses an 
internal combustion engine to provide power to its hydraulic systems and on- 
board computers. The ASV carries its own computers that control leg movement 
and stability, and provide vision information. The vision system consists of an 
optical radar mounted on top of the control cab. At present, the ASV requires an 
on-board human operator to plan and control the motion of its body using a 
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three-axis control stick. With continuing research, the ASV could become the 
first autonomous legged walking machine. [Ref. 1: pp. 1-60] 

C. DESCRIPTION OF OPTICAL RADAR 

1. Physical Description 

The ASV is equipped with an optical terrain scanner. The optical 
scanner is manufactured by the Environmental Research Institute of Michigan 
(ERIM). The scanner is 26 inches wide, 12.5 inches high and weighs 75 pounds 
[Ref.l: pg. 35]. The optical scanner consists of a scanning mechanism, transmitter 
optical train, and receiver optical train. The scanning mechanism consists of a 
nodding mirror and a four-sided polygon mirror which combine to scan up and 
down and left and right. The transmitter optical train consists of a GaAlAs laser 
diode, collimating lens, anamorphic prism pair and a beam expansion telescope. 
[Ref. 7: pp. 3-4]. 

2. Performance Data 

The optical scanner has a scan rate of 2 Hertz. The horizontal field of 
scan is +40 to -40 degrees in 128 increments while the vertical field of scan is from 
-15 to -75 degrees elevation in 128 increments. The instantaneous field of view is 
0.5 degrees and the range resolution is 0.125 feet. The maximum horizontal range 
is 32 feet. [Ref. 7: pp. 2-3] 

Due to various factors such as weather, obstacles and equipment errors, 
the range value returned by the ERIM scanner is not exact [Ref. 7: pg. 5]. The 
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ASV has or will have gyro or magnetic compasses for azimuth, elevation and roll 
inputs along with an altimeter for altitude information and geoposition satellite 
(GPS) inputs for horizontal position. Even though the ASV will be able to 
receive position inputs from GPS and the altimeter, they are inaccurate. That is, 
although the gyro’s give angles within fractions of a degree, the errors associated 
with GPS and the altimeter are of the order of tens of feet; therefore, x, y, and z 
need to be corrected. Detailed information regarding these two sources of terrain 
mapping error was not available at the time of writing of this thesis. 

D. ESTIMATION TECHNIQUES 
1. Regression Analysis 

In regression analysis, a number of noisy measurements of a variable are 
used to approximate a functional relationship. Using the notation of Ref. 8, if the 
measured variable of the system is 

»o(0 “ + «(0 (2- 1 ) 

then the objective of regression analysis is to find a vector, c , which approximates 
the true parameter vector, c 0 , in the presence of the measurement error, e . 
Typically, this is accomplished by minimization of some type of sum squared error 
function. 

If the response of the system under consideration can be represented 
linearly els 
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y 0 = A ? 0 + ? 



( 2 . 2 ) 



where y 0 is a vector of samples of y 0 , A is the coefficient matrix, c 0 is the true 
parameter vector, and t is a random error vector, then the sum squared error 
function , $, can be defined as 

* = fy 0 _ a ^) T (vo ~ AZ ) ( 2 - 3 ) 

where c is the trial parameter vector. To minimize $ , the partial derivatives of $ 
with respect to each component of c are equated to zero. Applying this to Eq. 
(2.3), the result is 

0 $ -dr T T 

= V $ = -2,4 T y 0 + 2A'Ac=0 (2.4) 

3 c 

or 

A T Ac = A T y 0 ( 2>3 ) 

Solving Eq. (2.5) for the least squares estimate of the true parameter vector, the 
result is 

c = \A T A}~ l A T y 0 (2.6) 

This is the least squares parameter estimate of the true parameter, c. [Ref. 8: pp. 
65-68] 

2. Iterative Methods 

The Kalman filter is an iterative method used to obtain an optimal 
estimate of a variable in an environment that has noise present. Unlike regression 
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analysis, in using a Kalman filter, it is not necessary to store past measurements 
for present or future computations. For the purposes of this discussion, the 
notation in Ref. 9 is used. It is assumed the reader has some knowledge of 
Kalman filtering. 

Assume a system is described by 

H = + a k- 1 ( 2 - 7 ) 

and 

h = H k*k + v k ( 2 - 8 ) 

where x k is the state variable at time t k , $ k _ k is the transition matrix at time « 4 _ 1 , 
w k _ k and v k are the random noise vectors with zero mean and covariances Q k and 
R k , respectively, z k are the measurements, and H k is the observation matrix. An 
updated estimate of the state, £*(+), based on the measurement z k and the past 
estimate, ?*(-), can be obtained from the recursive form 

^*(+) = Kk*k (~ ) + Kk^k ( 2> ^) 

where K k and K k are the time-varying weighting matrices (Kalman gain matrices). 

Ref. 9: pp. 60-110] 

If i k denotes the estimation error, such that 





( 2 . 10 ) 


^k(~) = ^ *ki~) 


( 2 . 11 ) 



then Eqs. (2.8-2.11) yield 
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**(+) = \K k + W k - l\l k + K k i k (~) + K k V k . 



( 2 . 12 ) 



By definition, = 0 and if E[2 k (-)\ = 0, this estimator will be unbaised for 
if 



K + K k H k — 7 = 0 . 
Thus, Eqs. (2.10) and (2.12) become 



**(+) = **H + K k \z k - H k i k {-)\ 



and 



2*(+) = (/- K k H k )i k (~) + K k v k . 



By definition, the error covariance matrix, P k , is given by 



P*(+) = £&(+)?*(+) r ] 

and 



PkH = E[i k H^k(-) T \ • 



If it is assumed that the measurement errors are uncorrelated, then 



£fo(-R r l = £[M*(-) r l = 0 • 



Applying Eq. (2.12) to Eqs. (2.16) and (2.17) 



jy+) = (/ - K k H k )P k (-)(I - K k H k ) T + K k R k K k . 



The Kalman gain matrix, K k , is defined as 



K k = P k (-)H T k \H k P k (-)H T k + R k 



Substituting Eq. (2.20) into Eq. (2.19) 



any x k 

(2.13) 

(2.14) 

(2.15) 

(2.16) 

(2.17) 

(2.18) 

(2.19) 

( 2 . 20 ) 
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p,l+) - 1; - . 



( 2 . 21 ) 



From Eqs. (2.14), (2.20) and (2.21), a recursive filter can be set up to obtain the 
optimal estimates of f t (+) [Ref. 9: pp. 60-110]. 

E. SUMMARY 

As has been discussed in this chapter, there have been in the past and are 
now in progress, many projects to develop autonomous vehicles. To become 
autonomous, such a vehicle ought to have a vision system and one such system 
was described. In the next chapter, methods for handling the vision information 
are applied to the problem addressed by this thesis. 
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III. PROBLEM STATEMENT AND PROPOSED SOLUTION METHOD 



A. INTRODUCTION 

Before a vehicle can become autonomous, it needs to know what the terrain it 
is operating in looks like then be able to pick a path it wants to travel along. In 
this chapter, some path selection techniques are discussed in general terms along 
with some detailed discussion on filtering of terrain data. 

For the remainder of this thesis, when reference is made to an optical radar 
scanner, the ERIM discussed in Section C of Chapter II is assumed to be the one 
in use. This assumption will show up in performance data assumed for the data 
conversions and simulation. 

B. IMMEDIATE- ARE A TERRAIN SENSING AND PATH SELECTION 

The need for information about the terrain in the immediate area of a 
walking machine in crucial. As long as the machine has a human driver, he can 
navigate the terrain manually, picking where to walk and, if necessary, placing 
the feet in appropriate positions. However, before a walking machine can become 
truly autonomous, it needs to be able to navigate on its own and pick its own 
path to walk along. Moreover, even in the case of a manned vehicle, it is highly 
desirable that the driver be concerned only with control of the body of the vehicle 
with foothold selection being accomplished automatically from vision data. 
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There have been many approaches and schemes for solving the problem of 
selecting adequate footholds and avoiding obstacles. The Autonomous Land 
Vehicle (ALV) uses a vision system that is a combination of a color video TV 
camera and the ERIM optical radar scanner [Ref. 5: pp. 19-23]. The color camera 
detects differences in color between the road and off-road areas, while the optical 
scanner is used strictly to detect differences in smoothness between the two areas. 
A major disadvantage of this system is that the color camera is greatly affected by 
changes in lighting and weather conditions. 

Another obstacle avoidance system is used in the FMC ALV. In that system, 
the Observer uses information from a sonic image sensor to detect obstacles not 
already known on the stored digitized map and then creates an obstacle map. 
The sonic image sensor is a phased array sensor that has a range of 32 meters and 
an arc of 120 degrees. The sensor utilizes sonic waves and produces a map every 
0.2 seconds. [Ref. 6: pp. 14-23] 

With a vision system consisting of just an optical radar terrain scanner, the 
problem of autonomous navigation becomes even more difficult. Before an 
"optimal" path can be picked to walk along, accurate information about the 
terrain altitude in the immediate area has to be known. Once this information is 
available, a scheme can be used to decide what areas can be walked over and 
what areas cannot. 

One possible terrain classification scheme was developed by Poulos in Ref. 10. 
In his work, a least squares quadratic surface is fitted to a surface pixel and its 



23 



eight neighbors. From the x, y, z information of each pixel, he uses the gradient 
vector and Hessian matrix and solves for the eigenvalues of the Hessian matrix. 
From these solutions, he classifies each pixel as a saddle, depression, ridge, plane, 
valley, hill or pass [Ref. 10: pp. 50-69]. To further refine the terrain picture, he 
also takes into account the slope associated with a pixel. Depending on what 
criteria is set for what is a safe slope to traverse, each pixel is also classified as 
being level or having a safe or unsafe slope. The property of being level, safe 
slope, or unsafe slope is called the primary terrain cell classification, while the 
above mentioned categories derived from the Hessian matrix are called the 
secondary classification [Ref. 10:pp. 70-78]. 

With each terrain cell classified, it becomes possible to use some type of 
artificial intelligence technique to select the "optimal" path to traverse [Refill]. 
The only problem is to make sure that the terrain information provided to the 
classifying scheme is the best and most accurate possible. 

C. TERRAIN SENSING USING THE OPTICAL RADAR 

To be able to use the information obtained from the optical scanner, the data 
must first be converted into Cartesian coordinates. This conversion of the data 
will allow manipulation of any prior or future information that is not in the same 
data format as the incoming data [Ref. 7: pp. 2-3]. In order to convert the optical 
scanner information into Cartesian coordinates, it is necessary to use not only the 
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scanner information, but also the information from the Inertial Navigation System 
(INS). 

To do the scanner data conversion, it was decided to model the INS and 
optical scanner systems as a nine link manipulator and to use the Denavit- 
Hartenberg (D-H) transformation for this purpose [Ref 12:. pp. 36-41]. The inputs 
from the INS system consist of an x, y, z translation from the INS origin, and the 
body azimuth, elevation and roll angles. The inputs provided by the optical 
scanner are scanner elevation and azimuth angles and range to the terrain. The 
first three links and the last link of the model are translational transformations, 
while the others are rotational transformations. Figure 1 is the representation of 
the resulting nine link manipulator. Table 1 explains what each value in Figure 1 
represents. 

In drawing Figure 1 and setting up the D-H transformation matrices, the 
following assumptions were made: [Ref. 12:pp. 36-41] 

1. The reference coordinate system for the ASV body is positive z down, positive 
x out of the front of the vehicle, and positive y out the right side. 

2. The z,_j axis lies along the axis of motion of the ith joint and the z, axis is 
normal to the z,_, axis. 

3. When possible, the direction of the ith twist axis, z,, associated with a 
particular link in the manipulator model is picked to make the twist angle 
(a,.) positive. 



25 



HAP COORDINATES 




Figure 1 

Nine Link Manipulator Representation 
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body azmuith 



TABLE 1 

D-H TRANSFORMATION SYMBOL MEANINGS 



Symbol 


Represents 




displacement from the INS origin in the z direction 


d* 


displacement from the INS origin in the x direction 


d. 


displacement from the INS origin in the y direction 


6 4 


ASV body azimuth angle 


6 S 


ASV body elevation angle 


6 « 


ASV body roll angle 


©T 


scanner elevation angle 


0 8 


scanner azimuth angle 




range to terrain 


“6 


distance between INS origin and scanner origin 


“7 


distance between scanner mirrors 



The following definitions are needed for the discussion of the D-H 
transformation matrices for link i: [Ref. 12:pp. 36-41] 

1. Link length (a,) is the linear displacement from the inboard motion axis to 
the outboard motion axis along the twist axis, (x,). 

2. Twist angle (a,) is the angular displacement of the outboard motion axis, (z ( ), 

from the inboard motion axis, about the twist axis, (z,). 

3. Joint displacement (d,) is the linear displacement of the outboard twist axis, 
(z,.), from the inboard twist axis, (x._,), measured along the inboard motion 
axis, 

4. Rotation angle (©,) is angular displacement of outboard twist axis,(z,), from 
the inboard twist axis, (z ( _,), measured about the inboard motion axis, 
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Using these assumptions and definitions, Table 2 shows the joint and link 
parameter values for the ASV scanner system. The values in Table 2 in 
parentheses are for the reference configuration shown in Figure 1 while the others 
are fixed values. 

Using the information in Table 2, the general D-H transformation matrix, Eq. 
(3.1), can be used to form each link as follows: 



c© ( - — ca^©, ac©- 

ca i cQ i — sa i cQ i asQ i 

1-1 i4 - = 

0 sa { CQ| d i 

0 0 0 1 

In Eq. (3.1), 11 A { is the D-H transformation matrix for link i going from origin (i- 
1) to origin i. Also, c© ( represents cos©,, and «©, represents sin©,-. To obtain the 



(3.1) 



TABLE 2 

LINK AND JOINT PARAMETER VALUES 



Link i 


©, 






a i 


1 


90 




90 


0 


2 


90 




90 


0 


3 


90 


dz 


90 


0 


4 


© 4 (180) 


0 


90 


0 


5 


e s (-90) 


0 


90 


0 


6 


0 6 (180) 


0 


90 


fl 6 


7 


G, (-90) 


0 


-90 


a 7 


8 


e. (-90) 


0 


90 


0 


9 


0 


d 9 


0 


0 
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Cartesian coordinates of the terrain at the end of the scanner beam, each 



transformation matrix is multiplied in order as shown in Eq. (3.2). 



° J 4 9 = °^4j x 1 A 2 x 2 A s x S A 4 x *A S x 5 j4 6 x 6 j4 7 x 7 A $ x *A g 



(3.2) 



In order to simulate the scanning of terrain on the graphics computer, the 
matrix °A 6 which specifies the position of the ASV body is computed and then 
multiplied by the matrix 6 4 9 to get the coordinate transformation for the beam 
tip. Using Eq. (3.1) and Table 2, the following results are obtained 



and 




c4c5c6-t-$4s6 c4«5 c4c5s6— s4c6 d 2 
«4c5c6— c4«6 $4*5 54c5«6-fc4c6 d 5 
$5c6 -c 5 «5«6 d x 

0 0 0 1 



(3.3) 



c7c8 — « 7 c7s8 d g c7s8 + a7c7 
s7c8 c7 t7e8 d g s7 $8+a7s7 

*.4 9 = 

9 -«8 0 c 8 d g c8 

0 0 0 1 

In Eqs. (3.1), (3.3), and (3.4), the notation has again been abbreviated so that, for 
example, c7 stands for cos© 7 and s7 stands for sin0 7 . Also, the origins of the 
scanner and the INS system are assumed to be in the same place, thus making a 6 
= 0. While this is in fact not the case for the ASV, this assumption simplifies the 
computations needed to evaluate Eq. (3.2) and at the same time has no effect on 
the validity of the simulation studies carried out in the work of this thesis. 
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D. PROPOSED METHOD OF DATA FILTERING 



Under the present scheme used by Ohio State University, the ASV scans the 
terrain using the optical radar scanner, and using whatever information the INS 
provides, a terrain map is calculated and stored. On all successive scans, the 
terrain map is recalculated with the given data, and the resulting new terrain 
elevation values replace the old ones. Each map is stored using the x, y, and z 
values calculated using the noisy range values provided by the optical radar 
scanner, which makes these x, y, and z values incorrect. The following discussion 
presents an approach to correcting for the z value errors separately from the x and 
y value errors. It is proposed to use Kalman filtering for the z values and 
regression analysis for the x and y values. 

1. Stationary Walker Case 

In the case where the ASV is standing still, the only significant errors 
introduced into the system are the noisy range values from the optical radar 
which in turn affect the calculated values of the altitude, z. In this case, to obtain 
the optimal values of the altitude for each terrain cell, Eqs. (2.14), (2.20) and 
(2.21) can be used. If the assumption is made that H k in Eq. (2.14) is equal to 1, 
then Eq. (2.14) becomes 

j.(+! - i.H + K, p; - !,(-}] (3.5) 

where #*(+) is the estimate of the terrain altitude after the range measurement. 
Also, f k (-} is the estimate before the measurement and K k is the Kalman gain 
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matrix. Finally, z k ' is the value of the altitude calculated using the noisy range 
measurement. 

From Eq. (2.20), P k (~) is defined as the variance of the error before the 
measurement and R k is the variance of the measurement. For the purposes of this 
problem, R k is the same as the variance of the altitude with respect to the range 
and P k (~) is the total variance of the altitude. Thus, R k can be written as 



R k = a] = (- 



02 



9 range 



\2 2 
") 



(3.6) 



where o] can is the variance associated with each scan of the optical radar scanner. 
In the case of P k (-), there is not only a variance in the z direction but there is also 
a variance in the x and y calculations due to the noisy range values. Thus, /**(-) 
can be written as 



AH = * 1 ,*, = + (-HX 

dx 9 y 



(3.7) 



Using Eqs. (3.6) and (3.7), Eqs. (2.20) and (2.21) can be written as 



At = <4,„H klal + 



(3.8) 



and 



A(+) = + °lto, <,/(—)! 1 • ( 3 - 9 ) 

2. Compensating for Walker Motion 

When the ASV is walking, or when there is an INS initialization error, 
the problem becomes one of computing the optimal value for the altitude and 

ensuring it is stored in the correct terrain cell. Due to the inaccuracies of the 
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altimeter and GPS inputs discussed in Section C of Chapter II, it is necessary to 
estimate the difference in the horizontal coordinates, Ax and Ay. For this part of 
the problem, regression analysis will be used to select the correct terrain cell. 
Then the optimal altitude value calculated from Kalman filtering will be stored. 
Two methods of regression analysis will be discussed for use in this problem: the 
gradient descent method and a grid search method, 
a. Gradient Descent Method 

In this approach, there is a criterion function, for a given set of 
parameters. The criterion function will be the sum squared error between the 
range returned by the optical radar scanner, R, and the range computed from the 
internal terrain map pre-stored in the ASV computers, R. From this, Eq. (2.3) 
can be written as 

$ = £(R - R) t (R - R) . (3.10) 

To correct for the x and y coordinate drift, the gradient of $, and 
the Hessian matrix, H, will be estimated using a 3 x 3 grid around the INS values 
of the x and y coordinates. Using the procedure described by Poulus in Ref. 10, 
the 3x3 terrain cell mask is set up with the cell of interest at point where x=0 
and y=0 with its associated value of <J> 0 . Table 3 shows the relative distances of 
the cells and the criterion functions associated with them. 

A quadratic function can be fitted to the cells of interest as a function 
of x and y which has the form 
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TABLE 3 

RELATIVE DISTANCES OF TERRAIN 
CELL MASK AND CRITERION FUNCTIONS 



II 

1 

h- ‘ 


X 

II 

o 


X 

II 

h-* 


y=i 


y=i 


II 

)— * 








1 

II 

X 


O 

II 

X 


X=1 


o 

II 


o 

II 


o 

II 

>> 


^4 






x=-l 


X 

II 

o 


X=1 


II 

1 


y=-l 


y=-i 


*6 


*7 





» 2 2 
R = fcj + k 2 x + k z y + k K z + k s xy + k^y 



(3.11) 



Eq. (3.11) can be written as 



^ 4 9i 6 ^6x1 



(3.12) 



where the subscripts are the dimensions of the matrices and 

k = (k, k, k s k t k s kf (3.13) 

and 

A , = 0 z , y< *i x ,y, y, 2 ) • ( 3 - 14 ) 

By substituting Eqs. (3.12-3.14) into Eq. (3.10) and taking the partial derivatives 
with respect to the Jfc ■ ' « , Eq. (2.4) becomes 
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( 3 . 15 ) 



9$ ip m ni 

= = -24 $ + 2 A Ak = 0 

3*,- 

or 

A T Ak = A T * T . (3.16) 

Rearranging terms, Eq. (2.6) can be written as 

k = [A T A]~ l A T * T . (3-17) 

The gradient of the terrain cell of interest is [Ref. 10: pg. 59] 

= + (3.18) 

and the Hessian matrix is [Ref. 10: pg.60] 



H = 



k 4 21j 



2^5 ^6 



(3.19) 



Applying Eqs. (3.18) and (3.19), the optimum amount of Ax and Ay 
can be computed as 

(Ai,Ay) = /T l v$ (3.20) 

which leads to the best estimate for the x and y coordinates of the terrain cell as 

optimum = i^INS + &X,Y, NS + Ay) (3.2l) 

where X, NS and ^ INS are the coordinates supplied by the INS system. 

In order to be able to compute the optimum values of Ax and Ay, the 
values of the k’s in Eq. (3.17) have to be computed. Equation (3.17) can be re- 
written as 
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where 



k = [ A t A } 1 A r $ T = £$ r 



(3.22) 



B = \A T A] l A T 



(3.23) 



To avoid repeated lengthy matrix computations, Ref. 10 show's B to be 



- 1/9 2/9 - 1/9 2/9 5/9 2/9 - 1/9 2/9 - 1/9 

- 1/6 0 1/6 - 1/6 0 1/6 - 1/6 0 1/6 

1/6 1/6 1/6 0 0 0 - 1/6 - 1/6 - 1/6 

1/6 - 1/3 1/6 1/6 - 1/3 1/6 1/6 - 1/3 1/6 

- 1/4 0 1/4 0 0 0 1/4 0 - 1/4 

1/6 1/6 1/6 1/3 - 1/3 - 1/3 1/6 1/6 1/6 



(3.24) 



b. Grid Search Method 

For this search method, the assumption is made that the criterion 
function, $, is definitely reduced when a sufficiently small step is taken in the 
direction of the actual terrain cell. Applying this to the terrain cell mask in Table 
3, the distances between each cell will be assumed to start at one standard 
deviation, a, of the INS system estimate of x and y instead of a unit 
displacement. After each computation of the $’s in the 3x3 mask, the least 
value $ is moved to the center of the mask and the coordinates of that terrain cell 
become the terrain cell of interest and the 3x3 mask and the associated <f>’s are 
again calculated w'ith each cell being displaced by a. Once the least value of <f> 
remains in the center cell for two successive searches, then the displacement of 



35 



each cell, A, becomes 



1 

A = (— ) a n = l,2,3 . (3.25) 

2 " 

The advantage of this type of search is that it does not depend on the 
cell of interest and its surrounding neighbors being able to be fitted with a 
quadratic function and thus can be applied to a larger variety of terrains. 

E. SUMMARY 

This chapter has presented the necessity for an accurate terrain altitude map 
before a vehicle can become autonomous. After the data from an optical radar 
scanner is converted into a form that can be used for route planning, there are 
various ways to compute the optimal altitude values and store them in the proper 
terrain cell. One method for computing the optimal altitude values was presented 
along with two ways to determine the proper terrain cell to store the altitudes. In 
the next chapter, the simulation model is presented that will be used to test these 
data manipulation methods. 
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IV. EXPLANATION AND JUSTIFICATION OF SIMULATION MODELS 



A. INTRODUCTION 

In Chapters II and III, various ways of handling data with INS and optical 
scanner errors were discussed. In order for these schemes to be tested, it is 
necessary to make use of either the ASV or similar vehicle, or an accurate 
computer simulation. The obvious choice for this thesis is the computer. In this 
chapter, a simulation model is presented that will allow simulation of terrain 
scanning by the optical' scanner and then data manipulation using the schemes 
presented earlier. 

B. TERRAIN AND OPTICAL SCANNER MODELS 

The computer chosen for the simulation studies of this thesis is an ISI 
Optimum V Workstation. This is a graphics workstation with an UNIX4.2BSD 
operating system. The workstation has a high-resolution display for a two- 
dimensional, black and white graphics display. The display is 1280 pixels wide 
and 1024 pixels high with the upper left corner of the screen functioning as the 
reference point. The operating system has installed libraries of graphics routines 
which are accessed by the computer language C, which is the language chosen for 
this thesis. These libraries provide graphics tools which make it easy to run a 
visual simulation. [Ref. 13] 
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In order to simulate the operation of the optical scanner, a simulated terrain 
is needed. The method for generating and drawing this terrain is the same as 
that presented in Ref. 10. An elevation oblique projection is used in this thesis to 
display a three-dimensional model of the terrain [Ref. 14: pp. 272-316]. In the 
elevation oblique representation, lengths and angles of lines in a vertical plane are 
preserved while others are distorted. Figure 2 shows the coordinate systems used 
for the Cartesian coordinate system, the ISI screen coordinate system, and the 
image coordinate system. Using these coordinate systems, the following equations 
apply: 



u = x — .5 y 


(4.1) 


V = — z — .5 y 


(4.2) 


X = X origin + U 


(4.3) 


Y = Y origin ~ V 


(4.4) 



The use of the multiplier value of 1 in Eqs. (4.1) and (4.2) means that each pixel 
in x and z corresponds to one inch in physical units. The use of a scale factor of 
.5 for the y coordinate in both of the equations means that y is foreshortened by a 
factor of .707 relative to the scale of x and z. With these scale factors, the terrain 
shown in Figure 3 is of dimension 64 ft. x 64 ft. With Eqs. (4.3) and (4.4), the 
reference of the terrain is set in the lower left corner of the terrain. 
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Figure 2 

Coordinate Systems 
a) Cartesian , b) Screen , c) Image 
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Simulated Terrain 
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The terrain data is generated by 



z 



\/(x 2 + y 2 ) 

r 4 cos( 2 n r3) 
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(4.5) 



where r3 and r4 are random numbers and x and y are the horizontal coordinates 
of the altitude map measured in inches. Figure 3 is a picture of typical terrain 
generated by Eq. (4.5). 

Figure 4 is a flow chart for the simulation program. The computer code in 
Appendix A is commented for further explanation of each segment of code. 
Figure 5 is a flow chart of the simulation of the optical scanner scanning the 
terrain. In scanning the terrain, the optical scanner’s beam length is increased 
one pixel (one inch) at a time. After each increase of the beam, the x, y, and z 
coordinates of the end of the beam are calculated using the D-H transformation 
discussed earlier. The calculated value of z is then compared to the value of the 
altitude stored at the x and y coordinate of the actual terrain data. If the 
calculated value of z is greater than the actual terrain value, the beam length is 
increased and the process starts again. If the calculated value is less than the 
actual terrain value, then the optical scanner’s range is set to 



R = ~ 5 (4-6) 

where R ieam is the range to the z value that is less than the actual terrain value 

and the .5 adjustment brings the range back to a value closer to the actual range. 

The actual ERIM optical scanner scans the terrain in 128 increments, both in 

elevation and azimuth. However, due to the time requirements of simulating the 
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Figure 4 

Simulation Flow Chart 
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Figure 5 

Scanning Flow Chart 
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scanning process, the simulation of this thesis uses twelve increments of one 
degree each in both the azimuth and elevation. 

It is assumed that the error associated with the the optical scanner is range 
dependent; that is, the greater the range to the surface, the greater the error of 
the returned range value. For the purposes of this thesis, this error will be 
represented by 



2 

&$can 



2 , 2 d 2 
< 7 0 + o l R 



(4.7) 



where o] can is the total variance of the scan and R is the range from Eq. (4.6). 
Also, lacking any information about the scanner errors, a 2 0 and a\ are set at 

<7g = .02 in (4-8) 



and 



o j = .04 . (4*9) 

After the variance in the range measurement is computed, the new value of 
the range, R, is computed by 

R-R + (4.10) 

where n is the output of a gaussian random noise generator with zero mean and 
unit variance and 



a 



• can 



= \A 



•2 

•can 



(4.11) 



44 



C. DATA HANDLING DURING SIMULATION 



Once the terrain has been scanned by the optical scanner, regression analysis 
is applied to compute any errors in the INS values for the x and y coordinates. 
Figure 6 is a flow chart of the gradient search method of regression analysis. 
Using the INS values of the x and y coordinates, $ 0 is computed using Eq. (3.10). 
The INS value of the x and y coordinates are then changed by the values in Table 
3 and the associated $’s are computed. To compute the $’s, the terrain is 
scanned, but this time without noise added to the range value. In the actual ASV, 
this would be done internally with the pre-stored terrain map of the area and not 
an actual scan of the terrain. From these values of $, Eqs. (3.22) and (3.19) are 
used to compute the k’s and the H matrix. With these, Ax and Ay are 
determined by Eq. (3.20). With these values of Ax and Ay, the INS values for x 
and y can be adjusted by Eq. (3.21). 

The second regression analysis method is the grid search method. Figure 7 is 
the flow chart for this search method. It is much the same as the gradient search 
method except that the INS values of the x and y coordinates are changed by the 
standard deviation, <x, of the INS estimate of x and y. After each cell’s $ is 
computed, the minimum value of 4> is determined, and if it is not in the center 
cell, then it is moved to the center along with its associated value of x and y. If 
the minimum 4> is already in the center cell, it is left there. If the minimum $ 
was not in the center cell, then after it is moved to the center, the x and y values 
are again changed by a. This process continues until the minimum $ stays in the 
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Figure 6 

Gradient Search Flow Chart 
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center cell for two successive iterations. Once this condition is satisfied, the x and 
y values are changed by a/2 n and the $’s are again computed. This process 
continues for n=l,2,3. Upon completion of the search method, the cell with the 
minimum 4> has the new INS value of x and y to replace the old values. 

After completion of the regression analysis, the Kalman filter is used to store 
the optimal values of the altitude in the appropriate altitude map cell. The cell 
indices are determined using the new INS values for x and y determined by 
regression analysis. Figure 8 is the flow chart for the Kalman filtering. For 
initialization of the Kalman filter, if a cell in the altitude map does not have a 
stored value for a 7 oU , then the a 7 oU of that cell is set to be ( 120 ) 2 , which is the square 
of the vehicle’s height. From Eq. (3.7), the partial derivatives with respect to x, 
y, and z are dependent on the nature of the terrain. Eqs. (4.12) and (4.13) show 
that a\ and a 7 are given by 



2 



a 




(4.12) 



a 



2 

v 




(4.13) 



For purposes of this thesis, the values of < 7 2 and a 7 are set to 1 , but in reality, they 
are dependent on range as well as azimuth and elevation scan angles. 

From the terrain data generated for this simulation, an average value for 
dz 2 

( — ) was determined from the difference in the altitude value in the x direction 
dx 
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Figure 7 

Grid Search Flow Chart 
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Figure 8 

Kalman Filtering Flow Chart 
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with the y value held constant. From these calculations, the average value of this 
quantity is 



( — ) 2 = .116 in . 
dx 



(4.14) 



dz 2 . dz 2 

The average value for ( — ) was calculated in the same manner as that for ( — ) 



dy 



dx 



with the results the same as those in Eq. (4.14). 



Next, 



is set to be 



a\ = sin 2 0 T o\ can . (4.15) 

With the value of a 2 0(0/ computed, Eqs. (3.5) and (3.9) are applied to each cell of 
the altitude map to store the optimal value of the altitude. Upon completion of 
the filtering, the program returns to scanning the terrain. 



D. SUMMARY 

In this chapter, the computer simulation was discussed. The creation and 
scanning of the simulated terrain and the scanning of it was presented along with 
the two regression analysis schemes and the Kalman filtering scheme for handling 
the data obtained from the scanning. In the next chapter, the results of the 
simulation runs will be discussed. 
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V. SIMULATION PERFORMANCE 



A. INTRODUCTION 

In the previous chapters, methods for correcting INS drift errors were 
presented that might contribute to an effort to make the ASV an autonomous 
vehicle. After development of a computer simulation model in Chapter IV, 
numerous simulation runs were needed to validate not only the model but the 
various schemes for handling data so that an accurate terrain altitude map can be 
developed for future use in possible route planning. In this chapter, simulation 
runs are planned and run for a stationary walker and a walker with movement 
involved. Along with the results, some conclusions about these results are 
presented. 

B. SIMULATION RESULTS 

1. Stationary Walker Results 

The first test of this thesis was to run the simulation using a stationary 
ASV. In this case, the walker is in a permanent position and it scans the same 
terrain over and over while performing Kalman filtering on the altitude 
information. Figure 9 shows the simulated terrain with the area being scanned 
indicated on it. In this figure, the optical scanner is represented by a small square 
and there is a line drawn from the scanner down to the terrain to give the viewer 
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an idea of the scanner’s relative position over the terrain. Also, the INS origin is 
set to be the lower left hand corner of the terrain. Due to the assumptions made 
in Chapter III, Section C, the x coordinate increases positive to the right from the 
INS origin along the horizontal axis, and the y coordinate increases negative along 
the other (diagonal) axis. 

To evaluate the effectiveness of the Kalman filtering presented in this 
thesis, three terrain cells are looked at to see how the optimal altitude value 
changes as the terrain is scanned in a stationary position. To get a representation 
of how the filtering is working at different ranges, the three terrain cells are 
selected at scanner elevations of -26, -41, and -61 degrees. Figure 10 is a plot of 
the three terrain cells altitudes stored after each scan after Kalman filtering has 
been applied. To check that the filtering works no matter what the noisy range 
values returned by the optical scanner are, a second simulation was run on the 
same terrain in the same position. Figure 11 shows the results obtained for the 
same three terrain cells. 

The next test made on the stationary walker was done with the scanner 
in a different position on the terrain and pointing in a different direction. Figure 
12 shows the scanned terrain for this case. Figure 13 shows the results of the 
Kalman filtering on three terrain cells at the same elevation angles used in the 
first case. Figure 14 shows the same three terrain cells with different noisy optical 
scanner range values. 
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Figure 9 

Scanned Terrain 1 
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Figure 10 

Kalman Filtering Results, Run 
Stationary Walker on Terrain 
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Figure 11 

Kalman Filtering Results, Run 2 
Stationary Walker on Terrain 1 
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Figure 12 
Scanned Terrain 2 
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Figure 13 

Kalman Filtering Results, Run 1 
Stationary Walker on Terrain 2 
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Figure 14 

Kalman Filtering Results, Run 2 
Stationary Walker on Terrain 2 
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The last simulation conducted for the stationary walker case was to scan 
the same terrain shown in Figure 12 and let the scanner scan it for 40 scans. 
Figure 15 shows the results of the 40 scans for the same terrain cells looked at in 
Figure 13. 

2. Moving Walker Case 

The next case to look at was the walker with motion involved. To get 
the optimal altitude value for each terrain cell, errors in the INS values of the x 
and y coordinates must be corrected for so the altitude value can be stored in the 
correct cell. For the purposes of this thesis, the net effects of walker motion are 
represented by an INS initialization error or offset. 

The first method used to correct for the INS errors was the gradient 
descent method. The scanner was placed in the same position as represented in 
Figure 9 and given an initialization error of Ax = 5 inches and Ay = -4 inches. After 
the first scan, gradient descent was applied using Eq. (3.20), and the optimum 
values for Ai and Ay were 10 and 5 respectively. A second run was conducted 
with different INS errors and the results were again not close to the value they 
should have been. The reasons for this failure are not known, but appear to be 
associated with the inadequacy of a quadratic approximation to the squared error 
function, 4>. This problem might be solved by a more elaborate gradient descent 
method as discussed in Ref. 8, but this approach was not investigated in this 
thesis. 
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Figure 15 

Kalman Filtering Results After 40 Scans 
Stationary Walker on Terrain 2 
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After the apparent failure of the gradient descent method, the grid search 
method was applied to the moving walker case. The first simulation run using 
the grid search method was with the scanner in the position indicated in Figure 9 
and with the standard deviation of the INS offset given by, <7 = 6 inches. This a 
was an assumption and is used in the terrain cell mask described in Chapter III, 
Section D. To test the effectiveness of the grid search method, a run was 
conducted with the INS error less than a , another run with the error between a 
and 2<7, and a third run with the error greater than 2 a. Figure 16a shows the 
results of the regression analysis with the INS error less than a. The plot shows 
the actual x and y values, the starting point of the INS x and y coordinates and 
the numbers indicate the steps of the grid search. The final value is the value 
returned to be used in the Kalman filtering by the grid search at the end of the 
first actual terrain scan. As Figure 4 shows, the regression analysis is repeated 
each time a new frame of noisy range values is obtained from the simulation. 
Figure 16b shows the results of the grid search after the first scan with the INS 
error between a and 2 a. After the second scan, the data showed that the grid 
search method locked in on the actual x and y coordinates and stayed there for 
subsequent scans. Figure 17 shows the results of the grid search after the first 
scan with the INS error greater than 2<7. The data collected also shows that the 
grid search method would not lock in on the actual x and y values on the 
succeeding scans when the error was greater than 2 a. 
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Figure 16 

Grid Search Results With a = 6 inches 
a) INS Error < a , b) a < INS Error < 2 a 
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Grid Search Results With <7 = 6 inches 
INS Error > 2 a 
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Upon completion of each grid search, Kalman filtering was applied to 
obtain the optimal altitude value for each terrain cell. Figure 18 are the results of 
the Kalman filtering after 8 scans with the INS error less than a. The data also 
shows that the results of the Kalman filtering when the INS error is between a 
and 2 a are almost exactly the same as those shown in Figure 18. Figure 19 
presents the results of the Kalman filtering when the INS error is greater than 2<7. 

The next test of the grid search method was to set the value of 
a = 12 inches. With this value of a, the same type of runs were conducted as when 
<r = 6 inches. Figure 20 shows the results when the INS error is less than 2<7. Figure 
20a shows the grid search locks in on the actual x and y values with zero error 
after the first scan. Figure 20b indicates that the grid search did not lock in on 
the actual values after the first scan. To further test the grid search with the INS 
error between a and 2 <r, another run was conducted with different errors in that 
range and the results are shown in Figure 21. The data showed that the grid 
search locked in with zero error on the actual position on the first scan and stayed 
locked in on all succeeding scans. The final run was conducted with the INS error 
greater than 2<7 and those results are shown in Figure 22. As with the results 
when <7 = 6 inches , the grid search would not lock in on the actual position when 
the INS error was greater than 2<7. When the Kalman filtering data was reviewed, 
the results were very much like that received when <7 = 6 inches. 

So far all of the results for the moving walker case involved using a pre- 
stored terrain altitude map. One final simulation conducted was to see if the grid 
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Kalman Filtering Results With a - 6 inches 
INS Error < 2 a 
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Figure 19 

Kalman Filtering Results With a = 6 inches 
INS Error > 2a 
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Grid Search Results With a = 12 inches 
a) INS Error < a , b) a < INS Error < 2 a 



y 

pos i t 
( l riches) 



* pos it ( i nc hes) 

275 285 295 305 




Figure 21 

Grid Search Results With a = 12 inches 
a < INS Error < 2 a 
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Grid Search Results With a = 12 inches 
INS Error > 2 a 
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search method would work without a pre-stored map. In this case, the scanner 
was placed in the position represented in Figure 9 and the terrain was scanned 20 
times to obtain an altitude map. From previous results, it is known that the map 
stored for the area scanned was very close to the actual map and the data for this 
case also showed that to be true. Upon completion of the 20 scans, the scanner 
was given an offset and then regression analysis and Kalman filtering was applied 
for the next 8 scans. Figure 23 shows the results of the grid search after the first 
scan. As can be seen, the grid search went farther away from the actual position. 
Actually, the grid search only conducted 3 steps because of the safety factor built 
into the program so it would not look forever if it was not converging. Also, the 
data indicated that on the succeeding scans, the grid search actually got worse. 
This run was conducted with an a = 6 inches and the offset between a and 2 a. It 
was run again with the offsets less than a and the results were the same. 

C. CONCLUSIONS 

The first general conclusion to be made from the results presented in this 
chapter is that the Kalman filtering is working. As can be seen from the results of 
the stationary walker case, the stored altitude values were very close to the actual 
values. For the terrain cells where the scanner was at -26 and -61 degrees, the 
stored values were within .25 inch of the actual values while the one at -41 
degrees was within about 1.1 inches. To evaluate the significance of these results, 
data was obtained concerning the standard deviation of the raw terrain elevation 
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values obtained from the noisy range values. The observed standard deviation 
had a value of 2.95 inches when averaged over all cases, much greater than the 
filtered values. One possible reason why the stored values at -41 degrees had a 
greater error was the arbitrary range adjustment from Eq. (4.6) and the sin 2 effect 
of Eq. (4.15). In any case, these results would probably be good enough for some 
route planning program to use. 

Referring to Figure 15, an apparent bias in terrain altitude estimates is 
revealed. From the magnitude and general behavior of this bias, it is believed 
that it results from the quantization of range into increments of one inch. This 
quantization is modulated by the sin 2 term of Eq. (4.15), leading to the differences 
between the three curves of Figure 15. 

The next conclusion is that the gradient descent method of regression analysis 
was not effective with this simulated terrain. The reason for this failure is not 
known except it may have something to due with the terrain and the resulting 
"lumpy'’ error criterion function, 4>. 

The overall conclusion about the grid search method is that it is effective. 
How well it works depends on the a selected and the amount of INS error relative 
to that a. As Figure 20b showed, with a INS error of Ax = 21 inches and 
Ay = 19 inches , the grid search would not lock in on the actual position but as seen 
in Figure 21, with an error of Ax = 18 inches and Ay = -17 inches, the grid search 
locked in on the actual position. This would indicate that there might be a limit 
to the amount of INS error involved before the grid search can correct for the 



72 



error which is independent of the value of a. With this method though, the INS 
initialization errors or offset can be corrected for, within limits, and thus allowing 
the Kalman filtering to provide an accurate terrain altitude map when a pre- 
stored altitude map is available. 

Finally, using the grid search method without a pre-stored altitude map did 
not work. In analyzing the results, a possible reason for its failure was because 
the altitudes in all of the surrounding terrain cells was initialized to zero so when 
the terrain mask was applied and the stored map scanned, the altitude values 
stored in the resulting Cartesian coordinate map would probably be less than they 
should have been, thus giving a minimum criterion function in a false cell during 
regression analysis. With this false minimum, the grid search would chase after 
the wrong cell. 
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VI. SUMMARY AND CONCLUSIONS 



A. RESEARCH CONTRIBUTIONS 

In this thesis, a first approach was presented in providing an optimal terrain 
altitude map that could be used for route planning for an autonomous walking 
machine. Toward that end, an effective Kalman filtering scheme was developed 
and tested. The results of those tests show that the altitude values stored in the 
terrain map are good enough for a walking machine to walk across. 

Another important part of providing an optimal altitude map is to ensure 
that the altitude values are stored in the right terrain cell locations associated 
with the proper x and y coordinates of the terrain area being traversed. If the x 
and y coordinates are in error because of errors in the inertial navigation system, 
then it is necessary to correct for those errors before storing the output of the 
Kalman filter. To solve this problem, a regression analysis approached called the 
grid search method was presented. This search method proved to be effective in 
correcting for these land navigation errors on the simulated terrain within limits. 
These limits appear to be within about 18 inches of the actual x and y coordinate 
values with the terrain used in this thesis. For a first approach, this type of 
accuracy is good and provides for a satisfactory starting point for refining this 
method. 
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Another contribution of this thesis was to provide a simulation for terrain and 
an optical terrain scanner scanning that terrain. This simulation was developed 
for the ISI graphics workstations and could be helpful for any further work that is 
conducted on these or similar workstations. 

B. RESEARCH EXTENSIONS 

Since the topics presented in this thesis represent a first attempt at optical 
radar data filtering with the ASV in mind, there are many areas that can be 
expanded upon. 

First, further research is needed into why the gradient descent method did not 
work. It is very possible that the reason has something to due with the simulated 
terrain used. Another extension that could be pursued would be with the grid 
search method. Along with having a decreasing mask in the grid search, an 
expanding mask could be developed if the minimum criterion function cannot be 
brought into the center of the grid. Also, further research is needed to determine 
the optimal value for the grid size to use in the mask and what limits there are on 
the errors that can be corrected. Along with this, the grid search method should 
be applied to completely different terrain and the optimal value of grid size 
determined and then compared to the one for this terrain to see if there is a 
relationship. Next, investigation into ways to use the grid search method to 
correct for errors introduced by the accelerometers while walking would allow the 
vehicle to continuously walk while the grid search method corrects its position. 
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Currently, under the scheme presented in this thesis, the vehicle would have to 
take a few steps, stop and correct its position, and then take a few more steps. 

Another important area that needs to be studied is the one of applying these 
techniques to the moving walker case where no pre-stored altitude map is 
available. As seen in Chapter V, the first attempt involving this situation did not 
work. Research into why it did not work and how to make it work will be 
invaluable to making the walking machine autonomous. 

The last area that should be studied is to apply the techniques that have 
proven successful in simulation to actual terrain with actual optical scanner data. 
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APPENDIX 



PROGRAM LISTING 



*************************************************** 

This is a computer simulation of an optical 
scanner scanning simulated terrain using 
regression anaylsis and Kalman filtering. 

This program is written in C for the ISI 
graphics workstations. 

This program is compiled by 

cc scan.c -ltools -lbm -lvt -lm 

by Mark D. Rickenbach 
1 August 1987 

**************************************************** 



^include <math.h> 

^include <vt.h> 

#include <stdio.h> 

^include <bitmap.h> 

^include <tools.h> 
struct BMD *bmd; 
int seedl; 

float seed,seed2,a0_9[4][4],seed3; 
main() 

{ 

static int u,v,u2,v2,scan,ii jj,v21,u21,rr,f,init,x,y,z,k; 
static int randl,rand2,rand3,rand4,xint,yint, safe, counter; 
static int bmyl,bmxl,bmx,bmy,fd,ij,kk,xx,yy,s,rang,l; 
static float sigscan,sig02,sigl2,array[l28][l28],d9,r[456]; 
static float n,sigzold2128][l28],noise(),sigscan2,elev,dx[9],dy[9]; 
static float xxl,yyl,zzl,d3min,gmin,zhat(l28][l28],zz[l28][l28]; 
static float w,tl,celev,melev,sigold2[l28][l28],sigz2[l28][l28]; 
static float th4,th5,th6,dl,d2,d3,th7,th8,g[8],rhat[456],d2min; 

> ************************************************** ****************** 

Open window, allocate bit map and set line discipline to graphics 

for graphics simulation ASV scanning terrain. 
******************************************** ************************* 



fd=OpenWindow(15,63,1220,664, ,l Terrain Map"); 

SetLineDisc(fd,TWSDISC); 

if ((bmd=BM_Allocate(1220,664))==0) 

{ printf( ,f unable to allocate bit map"); 

} 

BM_SetAddressing(bmd,l); 

BM_SetColor(bmd,l); 

BM_SetBColor(bmd,0) ; 
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/******************************************************************** 
Initialize various arrays to zero. 

********************************************************************* 



for(i=0;i<128;+-bi) 

{for(j=0-j<128;++j) 

{sigold2[i][j]=0; 

zz [ i ][j]=°; 

map z [i]|j]=0; 

sigz2[i][j]=0; 

} 

} 



/*** ************************************************************** 
Compute array of altitude values for terrain map. 
****************************************************************** 



counter=0; 
do { 

seed=0.0; 

seedl=0; 

seed2=0.0; 

randl=ran(2l); 

rand2=ran(2l); 

rand3=ran(l); 

rand4=ran(12l); 

for(i=0;i<128;+-bi) 

{ for(j=0y<128;++j) 

{x=randl-i; 
y=rand2-j; 
tl=(x*x) + (y*y); 
w=(sqrt(tl)/30.)*6.2832*rand3; 
array [j][i]=rand4*cos(w); 

} 

} 

counter=counter -f 1; 

} while (counter < 0); 



*************************************************************** 
Draw the base of the terrain map. 



/ 



xint=30; 

yint=542; 

BM SetPosition(bmd,xint,yint); 

BM_SetThickness(bmd,3); 

BM_PaintLine(bmd, 792,542); 

BM_PaintLine(bmd, 1176, 161); 

BM_PaintLine(bmd,414,161); 

BM_PaintLine(bmd,xint,yint); 

BM_SetThickness(bmd,l); 
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*************************************************************** 

Draw terrain to bit map using scheme developed by 
Poulus in his thesis. Each terrain cell will be 
represented by a 6 in. x 6 in. square. 

**************************************************************** 

i=0; 

j=0; 

loop: 

x=i*6; 

y =J* 6; 

z=array[j][i]; 

bmx=xtrfunc(&xint,&x,&y); 

bmy=ytrfunc(&yint,&y,&z); 

loopl: 

if (i== 127 && j==127) 

{ BM_SetThickness(bmd,2); 
bmy l=bmy-f-z; 

BM_PaintLine(bmd,bmx,bmyl); 

BM SetPosition(bmd,bmx,bmy); 

BM_SetThickness(bmd,l) ; 

BM_DisplayBitmap(fd,PAINTR ASTER, bmd, 0,0, 0,0, 1220, 664,0); 
goto Joop2; 

} 

if(j==127) 

(if(i==0) 

{ x=0; 
y=j*6; 

bmx=xtrfunc(&xint,&x,&y); 

bmy=ytrfunc(&yint,&y,&z); 

BM_SetPosition(bmd,bmx,bmy); 

} 

BM_SetThickness(bmd,2); 

bmyl=bmy+z; 

BM_PaintLine(bmd,bmx,bmyl); 

BM_SetPosition(bmd,bmx,bmy); 

BM SetThickness(bmd,l); 

i=i+l; 

x=i*6; 

y=j*6; 

z=array(j](i]; 

bmx=xtrfunc(&xint,&x,&y); 

bmy=ytrfunc(&yint,&y,&z); 

BM_PaintLine(bmd,bmx,bmy); 
goto loopl; 

} 

if(i== 127) 

{ BM_SetThickness(bmd,2); 
bmy 1— bmy-f z; 

BM_PaintLine(bmd,bmx,bmyl); 

BM SetPosition(bmd,bmx,bmy); 

BM_SetThickness(bmd,l); 
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j=j+i; 

y=j*6; 

x=i*6; 

z=array[j][i]; 

bmxl=xtrfunc(&xint,&x,&y); 

bmyl=ytrfunc(&yint,&y,&z); 

BM_PaintLine(bmd,bmxl,bmyl); 

i=0;“ 

goto loop; 

} 

if(i==0 || j==0) 

{ if(i==0 && j==0) 
{BM_SetThickness(bmd,2); 
BM_PaintLine(bmd,bmx,bmy); 
goto loop4; 

} 

x=i*6; 

y=j*6; 

z=array[j][i]; 

bmx=xtrfunc(&xint,&x,&y); 
bmy=ytrfunc(&yint,&y,&z); 
BM_SetPosition(bmd,bmx,bmy); 
BM J3etThickness(bmd,2).; 
bmyl=bmy-l-z; 

BM_PaintLine(bmd,bmx,bmy l) ; 
loop4: 

BM_SetPosition(bmd,bmx,bmy); 

BM_SetThickness(bmd,l); 

i=i+l; 

x=(i-l)*6; 

y=(j + l)*6; 

z=array[j+l]|i-l]; 

bmxl=xtrfunc(&xint,&x,&y); 

bmyl=ytrfunc(&yint,&y,&z); 

BM_PaintLine(bmd,bmxl,bmyl); 

BM_SetPosition(bmd,bmx,bmy); 

x=i*6; 

Y=j*6; 

z=array|j]|i]; 

bmx=xtrfunc(&xint,&x,&y); 
bmy=ytrfunc(&yint,&y,&z); 
BM_PaintLine(bmd,bmx,bmy); 
goto loopl; 

} 

i=i+l 5 



x=(i-l)*6; 

y=(j + l)*6; 

z=array[j+l][i-l]; 

bmxl=xtrfunc(&xint,&x,&y); 

bmyl=ytrfunc(&yint,&y,&z); 

BM_PaintLine(bmd,bmxl,bmyl); 

BM SetPosition(bmd,bmx,bmy); 
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x=i*6; 

y=j*6; 

z=&rray[j)[i]; 

bmx=xtrfunc(&xint,&x,&y); 

bmy=ytrfunc(&yint,&y,&z); 

BM_PaintLine(bmd,bmx,bmy); 
goto loopl; 
loop2: 

^**************************** **************************** 
Enter the starting LANS information from keyboard. 
********************************************************* 



printf(" enter x-coord. of ASV 0); 

scanf("%P’,&d2); 

printf( M enter y-coord. of ASV 0); 

scanf("9 : cr , ,<fed3); 

printf(" enter z-coord. of ASV 0); 

scanf^^cf^&dl); 

printf(" enter scanner body azmuith angle 0); 
scanf("%r , ,ifct4); 

printf( n enter scanner body elevation angle 0); 
scanf( n /2P\&t5); 

printf(” enter scanner body roll angle 0); 

scanf("%P\&t6); 

scan=0; 

xxl=d2; 

yyl=d3; 

zzl=dl; 

d2=d2+29; /* Introduce LANS initialization errors if any */ 
d3=d3-30; 

********************************************************* 
Start scanning terrain. 

********************************************************** 



do { 

scan=scan + l; 
printf( M scan %d ”,scan); 
u2=(xxl-0.5*yyl+30); 
v2=(zzl + .5*yy 1+542); 

**************************************************************** 

Draw a rectangle to indicate position of the ASV scanner. 
***************************************************************** 



BM_SetPosition(bmd,u2,v2); 

BM_SetAddressing(bmd,0); 

BM_PaintRectangleInterior(bmd,8,8); 

BM_DisplayBitmap(fd,PAINTRASTER,bmd, 0,0, 0,0, 1220, 664,0); 
BM_SetAddressing(bmd,l); 

BM_SetThickness(bmd,2); 
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u21=xxl-.5*yyl+30; 
v21=.5*yy 1+542; 
BM_PaintLine(bmd,u21,v2l); 
BM_SetPosition(bmd,u2,v2); 
BM_SetThickness(bmd,l); 



Scan terrain with one full azmuith scan for each elevation 
increment starting at elevation representd by elev and 
scanning down. For init=l, this simulates scanner operation 
returning a noisy range value, r [rr] . For init=2, this 
simulates the ASV’s computer internally scanning its 
pre-stored terrain map and returning noiseless range values, 
rhat[rr]. For the purposes of this thesis, the scanner 
scans in three elevation segments of 12 degrees each at 
-20, -35, and -55 degrees. 

***************************************************************** 

for(init=l;init<3;init++) 

{rr=0; 

i=i; 

do { 

if(i==l) 

{elev=(-l)*20.; 

} 

else if (i==13) 

{elev=(-l)*22.; 

} 

else if (i==25) 

{elev=(-l)*30; 

} 

th7=elev-i; 

tt=l; 

j=i; 

do { 

rr=rr+l; 
th8=6-j; 
if (tt= — 1) 

{rang=0; 
do { 

rang=rang+l; 

d9=rang; 

computematrix(&xxl,&yyl,&zzl,&th4,&th5,&th6,&th7,&th8,&d9); 

l=(a0_9[l][4]/6); 

k=(-l)*a0_9[2][4]/6; 

celev=(-l)* (a0_9[3][4]); 

melev=array[k][l]; 

} while(celev>melev); 

} 

else 

{ 
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do { 

rang=rang-l; 

d9=rang; 

computematrix(&xxl,&yyl,&zzl,&th4,&th5,&th6,&th7,&th8,&d9); 

l=(a0_9[l][4]/6); 

k=(-l)*a0_9[2][4]/6; 

celev=(«l)* (a0_9[3][4]); 

melev=array[k][l]; 

} while(celev<=melev); 
do { 

rang=rang+l; 

d9=rang; 

computen^trix(&xxl,&yyl,&zzl,&th4,&th5,&th6,&th7,&th8,&d9); 

l=(a0_9[l][4]/6); 

k=(-l)*a0_9[2][4)/6; 

celev=(-l)* (a0_9[3][4]); 

melev= array [k] [1]; 

} while(celev>melev); 

} 

d9=d9-.5; /* back up range by .5 inch */ 

u=(a0_9[l][4]-0.5*a0_9[2][4] + 30); 
v=(0.5*a0_9[2][4]-ha0_9[3][4]+542); 

BM_.SetPosition(bmd,u2,v2); 

BM_PaintLine(bmd,u,v); 

BM_DisplayBitmap(fd,PAINTRASTER,bmd, 0,0, 0,0, 1220,664,0); 



*************************************************************** 

Compute the noisy range value for init=l. 
**************************************************************** 



sig02=0.02; 

sigl2=0.0001; 

n=noise(); 

sigscan2=sig02 -f sigl2*d9*d9; 
sigscan=sqrt(sigscan2); 
if(init ==1) 

{n=noise(); 
d9=d9+n*sigscan ; 
r[rr]=d9; 

} 

else 

{rhat[rr|=d9; 

} 

tt=tt+l; 

j=j+i; 

} while(j<12); 
i=i+l; 

} while(i<36); 

} 
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^******************************************************** 
Start regression analysis. Set value of sigma (s) to 

be used in the cell masks. 

********************************************************** 



s= 12 ; 

g|0]=0; 

f=i; 

for(ii=l;ii<433;ii++) /* compute initial criterion function value, g[0] */ 
{g|0]=g[0] + (r[ii]-rhatjii])*(r[ii]-rhat[ii]); 

} 

d2min=d2; 

d3min=d3; 

gmin=g[0]; 

safe=l; 

kk=l; 

I****** ******************************** ********************* 

Change x and y coordinate values and simulate terrain 
scanning to obtain rhat[rr] to be able to compute 
criterion function values for each grid position. 

************************************************************ j 



do { 

d2=d2min; 

d3=d3min; 

g[0]=gmin; 

for(ii=l;ii<9;ii++) 

{if(ii==l) 

{d2=d2-s/f; 

d3=d3-s/f; 

} 

else if (ii==2) 
{d3=d3-s/f; 

} 

else if (ii= = 3) 
{d2=d2+s/f; 
d3=d3-s/f; 

} 

else if (ii= =4 ) 
{d2=d2-s/f; 

} 

else if (ii==5) 
{d2=d2+s/f; 

} 

else if (ii==6) 
{d2=d2-s/f; 
d3=d3+s/f; 

} 

else if (ii==7) 
{d3=d3-hs/f; 

} 
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else if (ii==8) 

{d2=d2+s/f; 

d3=d3+s/f; 

} 

it=0; 

i=i; 

do { 

if(i==i) 

{elev=(-l)*20.; 

} 

else if (i==13) 

{elev=(-l)*22.; 

} 

else if (i==25) 

{elev=(-l)*30.; 

} 

fch7=elev-i; 
tt— 1; 

j=i; 

do { 

rr=rr+l; 

th8=6-j; 

if(tt==l) 

{rang=0; 
do { 

rang=rang+l; 

d9=rang; 

computematria(&dl,&d2,&d3 1 &t4,&t5,&t6,&th7,&th8,&d9); 

l=(a0_9[l][4]/6); 

k=(-l)*a0_9|2]|4]/6; 

celev=(-l)* (a0_9(3][4]); 

melev=array[k][l]; 

} while(celev>melev); 

} 

else 

{ 

do { 

rang=rang-l; 

d9=rang; 

C0mputematrix(&dl,<kd2,&d3.&t4,&t5,&t6,&th7,&th8,&d9); 

l=(a0_9[l][4]/6); 

k=(-l)*a0_9[2j[4]/6; 

celev=(-l)* (a0_9[3][4;); 

melev= array[k][l|; 

}while(celev<=melev); 
do { 

rang=rang+l; 

d9=rang; 

computematrix(&dl,&d2,&d3,&t4,&t5,&t6,&th7,&th8,&d9); 

l=(a0_9[l][4]/6); 

k=(-l)*a0_9[2][4]/6; 

celev=(-l)* (a0_9[3][4j); 
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melev=axray[k][lj; 

} while (celev>melev); 

} 

d9=d9-.5; 

rhat[rr]=d9; 

tt=tt+l; 

j=j+i; 

} while(j<12) ; 
i=i+l; 

} while(i<36); 

y***************************************************** 

Compute criterion function for the appropriate 
cell mask. 

********************** ************************* ******* 



g[ii]=°; 

for(jj=lyj < 433yj++) 

{g[ii]=g[ii] + (r[jj]- r hatl[jj])*(r[jj]-rhatl[jjj); 

} 

dx[ii)=d2; 

dy[iij=d3; 

if(ii==l) 

{d2=d2+s/f; 

d3=d3+s/f; 

} 

else if (ii==2) 

{d3=d3+s/f; 

} 

else if (ii==3) 

{d2=d2-s/f; 

d3=d3+s/f; 

} 

else if (ii==4) 

{d2=d2+s/f; 

} 

else if (ii==5) 

{d2=d2-s/f; 

} 

else if (ii==6) 

{d2=d2+s/f; 

d3=d3-s/f; 

} 

else if (ii==7) 

{d3=d3-s/f; 

} 

else if (ii==8) 

{d2=d2-s/f; 

d3=d3-s/f; 

} 

} 
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******************************************************** 

Determine the minimum value of criterion function 

and its associated x,y coodinates. 
********************************************************* 



for(jj=l;jj<9;jj++) 
{if(gmin < g[jj]) 
{d2min=d2min; 
d3min=d3min; 
gmin=gmin; 

} 

else 

{d2min=dxj]j]; 

d3min=dy[jj]; 

gmin=g[jj); 

} 

} 



******************************************************** 

If minimum value of criterion function is not 
in center cell, keep f=l and increment safe by 1. 

If minimum is in center cell or safe=4, start 

increasing f by 2 times until its is > s. 
********************************************************* 



if(gmin != g[0 && kk==l && safe !— 4) 

{f=i; 

kk=l; 

safe=safe-bl; 

} 

else if (gmin == g[0] ) 

kk=kk+l; 

safe=4; 

} 

else if (safe == 4) 

{f=f*2; 

kk=kk-hl; 

} 

} while(f<s) ; 

d2=d2min; 

d3=d3min; 



*********************************************************** 

Start computation of x,y, and z values for each 
azimuth and elevation value. 

************************************************************ 



rr=0; 
i=l; 
do { 

if(i==i) 
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{elev=(-l)*20.; 

} 

else if (i==13) 

{elev=(-l)*22.; 

} 

else if (i==25) 

{elev=(-l)*30.; 

} 

th7=elev -i; 

j=i; 

do { 

th8=6-j; 

rr=rr+l; 

d9=r[rr]; 

computematrix(&xxl,&yyl,&zzl,&th4,&th5,&th6,&:th7,&;th8,&d9); 

xx=a0j9[l][4]/6; 

yy=(-l)*a°_9[2|[4|/6; 

zz[xx][yy]=(- 1 )*a0_9[3][4|; 

^*********************************************************** 
Perform Kalman filtering for each terrain 
cell scanned. 

************************************************************/ 



sigz2[xx][yy]=(sig02 -f sigl2*d9*d9)*sin(th7*3. 14159/180) 

* sin (th7*3. 14159/ 180) +.232; 
if(sigzold2(xx][yy]==0) 

{sigzold2[xx][yy]= 144000.; 

} 

zhat[xx][yy]=zhat[xx][yy] + (sigzold2[xx][yy|/(sigzold2[xx][yy]+sigz2[xx][yy])) 
*(zz[xx|[yy]-zhat[xx][yy|); 

sigzold2[xx][yy]=sigz2[xx][yy]*sigzold2[xx][yy]/(sigz2[xx][yy]+sigzold2|xx][yy]); 

j=j+i; 

} while(j<12); 
i=i+l; 

} while (i<36) ; 

} while(scan<8); 
printf(" program complete "); 
while (l) 

{} 

} 

^/ ********************************************************** 

End of the main program 

****?******************************************************/ 



^*********************************************************** 
Subroutine to perform the coordinate transformation 
using the D-H transformation. Only the values of the 
matrices needed are computed. 

************************************************************ 
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computematrix(xl,yl,zl,tth4,tth5,tth6,tth7,tth8,dd9) 
float *xl,*yl,*zl,*tth4,*tth5,*tth6,*tth7,*tth8,*dd9; 

{ 

float c4,c5,c6,c7,c8,thact4,thact5,thact6,th7,th8,d9; 

float s4,s5,s6,s7,s8,xxl,yyl,zzl; 

int a7; 

xxl=(*xl); 

yyl=(*yl); 

zzl=(*zl); 

thact4=(*tth4); 

thact5=(*tth5); 

thact6=(*tth6); 

th7=(*tth7); 

th8=(*tth8); 

d9=(*dd9); 

c4=cos(thact4*3. 1416/180 +3.1416); 
c5=cos(thact5*3. 1416/180 -1.5708); 
c6=cos(thact6*3. 1416/180 +3.1416); 
c7=cos(th7*3. 1416/180 -1.5708); 
c8=cos(th8*3. 1416/180 -1.5708); 
s4=sin(thact4*3. 1416/180 + 3.1416); 
s5=sin(thact5*3. 1416/180 -1.5708); 
s6=sin(thact6* 3. 1416/180 + 3.1416); 
s7=sin(th7*3. 1416/180 - 1.5708); 
s8=sin(th8*3. 1416/180 - 1.5708); 
a7=(-l)*5; 

a0_9[l][4)=xxl+(c4*c5*c6+s4*s6)*(d9*c7*s8+a7*c7) + (c4*s5)*(d9*s7*s8+a7*s7) 
+ (c4*c5*s6-s4*c6)*(c8*d9); 

a0_9[2][4]=yyl+(s4*c5*c6-c4*s6)*(d9*c7*s8+a7*c7)+(s4*s5)*(d9*s8*s7+a7*s7) 

+(s4*c5*s6+c6*c4)*(c8*d9); 

a0_9[3][4]=zzl+(s5*c6)*(d9*c7*s8+a7*c7)-(d9*s8*s7+a7*s7)*(c5) 

+ (s5*s6)*(c8*d9); 

} 



^/************************************************************* 
Subroutines to do x and y transformations used in the 

terrain drawing portion of the program. 
************************************************************** 



xtrfunc(xint,x,y) 
long int *x,*y ; 
int *xint; 

{ 

int xx; 

xx=(*xint)-h(*x)-h(*y)*.5; 
return xx; 

} 



ytrfunc(yint,y,z) 
long int *y,*z ; 
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int *yint; 

{ 

int yy; 

yy=(*yint)-(*y)*.5-(*z); 
return yy; 

} 



/**> ****************************************************** 

Random number generator used in creating terrain. 
********************************************************* 

ran(r) 
int r; 

{ 

int ran; 

seed=(seed2-b3.l) * (seed2+3.l); 

seedl=seed; 

seed2=seed-seedl; 

ran=(r )*(seed2) + 1; 

return (ran); 

} 



y* ******************************************* ************** 

Subroutine used to create a random gaussian number 
with mean=0. 

*********************************************************** 

float 

noise() 

{ 

float b,n,seed,x,xx; 
int i,seedl; 
b=0.5; 
xx=0; 

for(i=l;i< 13;H — hi) 

{seed=(seed3+3.141634)*(seed3+3. 141634); 
seedl=seed; 
seed3=seed - seedl; 
x=2*seed3 - 1; 
xx=xx + x; 

} 

n=b*xx; 

return(n); 

} 
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